﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using System.Runtime.InteropServices;
using System.IO;
using Microsoft.Kinect;

namespace jKinect.Tools
{
    public class SkeletonMath
    {
        public static SkeletonPoint Barycentric(params SkeletonPoint[] skeletonPoints)
        {
            SkeletonPoint bary = new SkeletonPoint();
            foreach (SkeletonPoint skeletonPoint in skeletonPoints)
            {
                bary.X += skeletonPoint.X;
                bary.Y += skeletonPoint.Y;
                bary.Z += skeletonPoint.Z;
            }
            bary.X /= (float)skeletonPoints.Length;
            bary.Y /= (float)skeletonPoints.Length;
            bary.Z /= (float)skeletonPoints.Length;

            return bary;
        }

        public static float Distance(SkeletonPoint skeletonPoint1, SkeletonPoint skeletonPoint2)
        {
            float x = skeletonPoint1.X - skeletonPoint2.X;
            float y = skeletonPoint1.Y - skeletonPoint2.Y;
            float z = skeletonPoint1.Z - skeletonPoint2.Z;
            float sq = x * x + y * y + z * z;
            return (float)Math.Sqrt((double)sq);
        }

        //public static float Distance(SkeletonPoint position1, SkeletonPoint position2)
        //{
        //    return Vector3.Distance(position1.ToVector3D(), position2.ToVector3D());
        //}


        ////public static Vector3 Barycentric(Vector3[] vectors)
        ////{
        ////    Vector3 bary = new Vector3();
        ////    foreach (Vector3 vector in vectors)
        ////    {
        ////        bary.X += vector.X;
        ////        bary.Y += vector.Y;
        ////        bary.Z += vector.Z;
        ////    }
        ////    bary.X /= (float)vectors.Length;
        ////    bary.Y /= (float)vectors.Length;
        ////    bary.Z /= (float)vectors.Length;

        ////    return bary;
        ////}
        //public static Vector Barycentric(Microsoft.Kinect.SkeletonPoint[] vectors)
        //{
        //    Vector bary = new Vector();
        //    foreach (Vector vector in vectors)
        //    {
        //        bary.X += vector.X;
        //        bary.Y += vector.Y;
        //        bary.Z += vector.Z;
        //    }
        //    bary.X /= (float)vectors.Length;
        //    bary.Y /= (float)vectors.Length;
        //    bary.Z /= (float)vectors.Length;

        //    return bary;
        //}
        //public static Vector3 Barycentric(Microsoft.Kinect.SkeletonPoint[] vectors)
        //{
        //    Vector3 bary = new Vector3();
        //    foreach (Vector3 vector in vectors)
        //    {
        //        bary.X += vector.X;
        //        bary.Y += vector.Y;
        //        bary.Z += vector.Z;
        //    }
        //    bary.X /= (float)vectors.Length;
        //    bary.Y /= (float)vectors.Length;
        //    bary.Z /= (float)vectors.Length;

        //    return bary;
        //}


    }
}
